using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;

namespace LocalizationTest
{
  public class MotionModel1D : MotionModel
  {
    public override bool StatesAreNearBy(WorldState oldState, WorldState newState)
    {
      return true;
    }
    public override double GetStateTransitionProbability(WorldState oldState, WorldState newState, ActionDescription action)
    {
      //ATTENTION!!! CHANGES IN THIS METHOD MIGHT REQUIRE CHANGES IN METHOD DoAction !!!
      return GetStateTransitionProbabilityForDiscreteAction(oldState, newState, action as DiscreteAction1D);
    }

    private static double GetStateTransitionProbabilityForDiscreteAction(WorldState oldState, WorldState newState, DiscreteAction1D action)
    {
      double movedDistance = (newState as WorldState1D).Position - (oldState as WorldState1D).Position;
      double deviationFromExpectedDistance = 0;
      if (action.typeOfAction == DiscreteAction1D.TypeOfAction.moveRight)
      {
        deviationFromExpectedDistance = movedDistance - Globals.motionRasterWidth;
      }
      if (action.typeOfAction == DiscreteAction1D.TypeOfAction.moveLeft)
      {
        deviationFromExpectedDistance = movedDistance + Globals.motionRasterWidth;
      }
      if (action.typeOfAction == DiscreteAction1D.TypeOfAction.doNothing)
      {
        deviationFromExpectedDistance = movedDistance;
      }

      if (Globals.selfLocatorProperties.MotionNoise == NoiseType.gaussianNoSystematic)
      {
        double probability = 0;
        if (action.typeOfAction == DiscreteAction1D.TypeOfAction.moveRight)
        {
          if (deviationFromExpectedDistance == -Globals.motionRasterWidth) probability = 0.0; //0.2
          else if (deviationFromExpectedDistance == 0) probability = 0.5; //0.7
          else if (deviationFromExpectedDistance == Globals.motionRasterWidth) probability = 0.5; //0.1
        }
        if (action.typeOfAction == DiscreteAction1D.TypeOfAction.moveLeft)
        {
          if (deviationFromExpectedDistance == -Globals.motionRasterWidth) probability = 0.1;
          else if (deviationFromExpectedDistance == 0) probability = 0.7;
          else if (deviationFromExpectedDistance == Globals.motionRasterWidth) probability = 0.2;
        }
        if (action.typeOfAction == DiscreteAction1D.TypeOfAction.doNothing)
        {
          if (deviationFromExpectedDistance == -Globals.motionRasterWidth) probability = 0.01;
          else if (deviationFromExpectedDistance == 0) probability = 0.98;
          else if (deviationFromExpectedDistance == Globals.motionRasterWidth) probability = 0.01;
        }
        return probability;
      }
      else if (Globals.selfLocatorProperties.MotionNoise == NoiseType.equal)
      {
        if (Math.Abs(deviationFromExpectedDistance) <= Globals.motionRasterWidth)
        {
          return 1.0 / 3.0;
        }
        else
          return 0;
      }
      else if (Globals.selfLocatorProperties.MotionNoise == NoiseType.noNoise)
      {
        if (deviationFromExpectedDistance == 0) return 1;
        else return 0;
      }
      else
        return 0;
    }
  }
}
